//
// Created by ZhaoXiaoFei on 2022/7/28.
//

#ifndef VINS_ESTIMATOR_PROJECTION_FACTOR_HPP
#define VINS_ESTIMATOR_PROJECTION_FACTOR_HPP
#include <ceres/ceres.h>
#include "../utility/utility.h"

class ProjectionFactor : public ceres::SizedCostFunction<2, 7, 7, 7, 1>{
public:
    ProjectionFactor(Eigen::Vector3d& _point_i, Eigen::Vector3d& _point_j) : point_i(_point_i), point_j(_point_j){

    }

    virtual bool Evaluate(double const* const* parameters,
                          double* residuals,
                          double** jacobians) const{
        Eigen::Vector3d Pi = Eigen::Vector3d(parameters[0][0], parameters[0][1], parameters[0][2]);
        Eigen::Quaterniond Qi = Eigen::Quaterniond(parameters[0][6], parameters[0][3], parameters[0][4], parameters[0][5]);

        Eigen::Vector3d Pj = Eigen::Vector3d(parameters[1][0], parameters[1][1], parameters[1][2]);
        Eigen::Quaterniond Qj = Eigen::Quaterniond(parameters[1][6], parameters[1][3], parameters[1][4], parameters[1][5]);

        Eigen::Vector3d Pbc = Eigen::Vector3d(parameters[2][0], parameters[2][1], parameters[2][2]);
        Eigen::Quaterniond Qbc = Eigen::Quaterniond(parameters[2][6], parameters[2][3], parameters[2][4], parameters[2][5]);

        double inv_dep_i = parameters[3][0];

        Eigen::Vector3d point_cam_i = point_i / inv_dep_i;
        Eigen::Vector3d point_imu_i = Qbc * point_cam_i + Pbc;
        Eigen::Vector3d point_w_i = Qi * point_imu_i + Pi;
        Eigen::Vector3d point_imu_j = Qj.inverse() * (point_w_i - Pj);
        Eigen::Vector3d point_cam_j = Qbc.inverse() * (point_imu_j - Pbc);
        double dep_j = point_cam_j.z();

        Eigen::Map<Eigen::Vector2d> residual(residuals);
        residual(0) = point_cam_j(0) / point_cam_j(2) - point_j(0);
        residual(1) = point_cam_j(1) / point_cam_j(2) - point_j(1);
//        residual = (point_cam_j / dep_j).head<2>() - point_j.head<2>();
        residual = sqrt_info * residual;

        if(jacobians){
            Eigen::Matrix<double, 2, 3> reduce;
            reduce << 1.0 / dep_j, 0, -point_cam_j(0) / (dep_j * dep_j),
                      0, 1.0/ dep_j, -point_cam_j(1) / (dep_j * dep_j);

            reduce = sqrt_info * reduce;

            if(jacobians[0]){
                Eigen::Map<Eigen::Matrix<double, 2, 7, Eigen::RowMajor>> jacobian_pose_i(jacobians[0]);
                jacobian_pose_i.setZero();

                Eigen::Matrix<double, 3, 6> tempJacobian;
                tempJacobian.setZero();
                tempJacobian.block<3, 3>(0, 0) = (Qbc.inverse() * Qj.inverse()).toRotationMatrix();
                tempJacobian.block<3, 3>(0, 3) = -(Qbc.inverse() * Qj.inverse() * Qi).toRotationMatrix() * Utility::skewSymmetric(point_imu_i);

                jacobian_pose_i.leftCols<6>() = reduce * tempJacobian;
                jacobian_pose_i.rightCols<1>().setZero();
            }

            if(jacobians[1]){
                Eigen::Map<Eigen::Matrix<double, 2, 7, Eigen::RowMajor>> jacobian_pose_j(jacobians[1]);
                jacobian_pose_j.setZero();

                Eigen::Matrix<double, 3, 6> tempJacobian;
                tempJacobian.setZero();
                tempJacobian.block<3, 3>(0, 0) = -(Qbc.inverse() * Qj.inverse()).toRotationMatrix();
                tempJacobian.block<3, 3>(0, 3) = Qbc.inverse().toRotationMatrix() * Utility::skewSymmetric(point_imu_j);

                jacobian_pose_j.leftCols<6>() = reduce * tempJacobian;
                jacobian_pose_j.rightCols<1>().setZero();

            }

            if(jacobians[2]){
                Eigen::Map<Eigen::Matrix<double, 2, 7, Eigen::RowMajor>> jacobian_pose_bc(jacobians[2]);
                jacobian_pose_bc.setZero();

                Eigen::Matrix<double, 3, 6> tempJacobian;
                tempJacobian.setZero();
                tempJacobian.block<3, 3>(0, 0) = Qbc.inverse().toRotationMatrix() * ((Qj.inverse() * Qi).toRotationMatrix() - Eigen::Matrix3d::Identity());

                tempJacobian.block<3, 3>(0, 3) = -(Qbc.inverse() * Qj.inverse() * Qi * Qbc).toRotationMatrix() * Utility::skewSymmetric(point_cam_i)
                                                                + Utility::skewSymmetric(Qbc.inverse() * Qj.inverse() * Qi * Qbc * point_cam_i)
                                                                + Utility::skewSymmetric(Qbc.inverse() * (Qj.inverse() * (Qi * Pbc + Pi - Pj) - Pbc));

                jacobian_pose_bc.leftCols<6>()  = reduce * tempJacobian;
                jacobian_pose_bc.rightCols<1>().setZero();

            }

            if(jacobians[3]){
                Eigen::Map<Eigen::Matrix<double, 2, 1, Eigen::ColMajor>> jacobian_inv_depth(jacobians[3]);
                jacobian_inv_depth.setZero();
                jacobian_inv_depth = -reduce * (Qbc.inverse() * Qj.inverse() * Qi * Qbc).toRotationMatrix() * 1.0 / (inv_dep_i * inv_dep_i) * point_i;
            }
        }
        return true;
    }
    Eigen::Vector3d point_i, point_j;
    static Eigen::Matrix2d sqrt_info;
};

#endif //VINS_ESTIMATOR_PROJECTION_FACTOR_HPP
